#include <iostream>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main()
{
    // define rotation matrix by angle-axis
    AngleAxisf r;
    r = AngleAxisf(0.25 * M_PI, Vector3f::UnitZ()) 
        * AngleAxisf(0.5 * M_PI, Vector3f::UnitY()) 
        * AngleAxisf(0.33 * M_PI, Vector3f::UnitX());

    // rotatation by angle-axis is the same to the one by quaternion
    Vector3f v(1, 0, 0); // Quaternionf(0, 1, 0, 0)
    Vector3f rotated_v = r * v;
    cout << "rotated by r: " << rotated_v.transpose() << endl;

    Quaternionf q = Quaternionf(r);
    rotated_v = q * v;
    cout << "rotated by q: " << rotated_v.transpose() << endl;

    // internal representation: [x, y, z, w]
    cout << "equals to: " << (q * Quaternionf(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

    // coeffes
    cout << "coeffes = " << q.coeffs().transpose() << endl;

    return 0;
}
